#include "eq_biquad.h"

int32_t biquad_float_to_q30_hex(double val)
{
    int64_t q30 = (int64_t)round(val * (1LL << 30));
    if (q30 < 0) {
        uint32_t hex = (uint32_t)(q30 & 0xFFFFFFFF);
        return (int32_t)hex;
    } else {
        return (int32_t)q30;
    }
}

void biquad_calcBiquad(int type, double Fc, double Q, double peakGain, double data[5])
{
    double norm;
    double V = pow(10, fabs(peakGain) / 20.0);
    double K = tan(M_PI * Fc);
    switch (type) {
        case bq_type_lowpass:
            norm = 1 / (1 + K / Q + K * K);
            data[0] = K * K * norm;
            data[1] = 2 * data[0];
            data[2] = data[0];
            data[3] = 2 * (K * K - 1) * norm;
            data[4] = (1 - K / Q + K * K) * norm;
            // 如果异常需要加这两行代码
            data[3] = 0 - data[3]; // -a1
            data[4] = 0 - data[4]; // -a2
            break;
            
        case bq_type_highpass:
            norm = 1 / (1 + K / Q + K * K);
            data[0] = 1 * norm;
            data[1] = -2 * data[0];
            data[2] = data[0];
            data[3] = 2 * (K * K - 1) * norm;
            data[4] = (1 - K / Q + K * K) * norm;
            data[3] = 0 - data[3]; // -a1
            data[4] = 0 - data[4]; // -a2
            break;
            
        case bq_type_bandpass:
            norm = 1 / (1 + K / Q + K * K);
            data[0] = K / Q * norm;
            data[1] = 0;
            data[2] = -data[0];
            data[2] = 2 * (K * K - 1) * norm;
            data[4] = (1 - K / Q + K * K) * norm;

            data[3] = 0 - data[3]; // -a1
            data[4] = 0 - data[4]; // -a2
            break;
            
        case bq_type_notch:
            norm = 1 / (1 + K / Q + K * K);
            data[0] = (1 + K * K) * norm;
            data[1] = 2 * (K * K - 1) * norm;
            data[2] = data[0];
            data[3] = data[1];
            data[4] = (1 - K / Q + K * K) * norm;

            data[3] = 0 - data[3]; // -a1
            data[4] = 0 - data[4]; // -a2
            break;
            
        case bq_type_peak:
            if (peakGain >= 0) {    // boost
                norm = 1 / (1 + 1/Q * K + K * K);
                data[0] = (1 + V/Q * K + K * K) * norm;
                data[1] = 2 * (K * K - 1) * norm;
                data[2] = (1 - V/Q * K + K * K) * norm;
                data[3] = data[1];
                data[4] = (1 - 1/Q * K + K * K) * norm;
                
                data[3] = 0 - data[3]; // -a1
                data[4] = 0 - data[4]; // -a2
            }
            else {    // cut
                norm = 1 / (1 + V/Q * K + K * K);
                data[0] = (1 + 1/Q * K + K * K) * norm;
                data[1] = 2 * (K * K - 1) * norm;
                data[2] = (1 - 1/Q * K + K * K) * norm;
                data[3] = data[1];
                data[4] = (1 - V/Q * K + K * K) * norm;

                data[3] = 0 - data[3]; // -a1
                data[4] = 0 - data[4]; // -a2
            }
            break;
        case bq_type_lowshelf:
            if (peakGain >= 0) {    // boost
                norm = 1 / (1 + sqrt(2) * K + K * K);
                data[0] = (1 + sqrt(2*V) * K + V * K * K) * norm;
                data[1] = 2 * (V * K * K - 1) * norm;
                data[2] = (1 - sqrt(2*V) * K + V * K * K) * norm;
                data[3] = 2 * (K * K - 1) * norm;
                data[4] = (1 - sqrt(2) * K + K * K) * norm;

                data[3] = 0 - data[3]; // -a1
                data[4] = 0 - data[4]; // -a2
            }
            else {    // cut
                norm = 1 / (1 + sqrt(2*V) * K + V * K * K);
                data[0] = (1 + sqrt(2) * K + K * K) * norm;
                data[1] = 2 * (K * K - 1) * norm;
                data[2] = (1 - sqrt(2) * K + K * K) * norm;
                data[3] = 2 * (V * K * K - 1) * norm;
                data[4] = (1 - sqrt(2*V) * K + V * K * K) * norm;

                data[3] = 0 - data[3]; // -a1
                data[4] = 0 - data[4]; // -a2
            }
            break;
        case bq_type_highshelf:
            if (peakGain >= 0) {    // boost
                norm = 1 / (1 + sqrt(2) * K + K * K);
                data[0] = (V + sqrt(2*V) * K + K * K) * norm;
                data[1] = 2 * (K * K - V) * norm;
                data[2] = (V - sqrt(2*V) * K + K * K) * norm;
                data[3] = 2 * (K * K - 1) * norm;
                data[4] = (1 - sqrt(2) * K + K * K) * norm;

                data[3] = 0 - data[3]; // -a1
                data[4] = 0 - data[4]; // -a2
            }
            else {    // cut
                norm = 1 / (V + sqrt(2*V) * K + K * K);
                data[0] = (1 + sqrt(2) * K + K * K) * norm;
                data[1] = 2 * (K * K - 1) * norm;
                data[2] = (1 - sqrt(2) * K + K * K) * norm;
                data[3] = 2 * (K * K - V) * norm;
                data[4] = (V - sqrt(2*V) * K + K * K) * norm;

                data[3] = 0 - data[3]; // -a1
                data[4] = 0 - data[4]; // -a2
            }
            break;
        default:

        break;
    }
    
    return;
}


filter_biquad_s32_t eq_peak[2] = 
{
    {
        .biquad_count = 1,
        .state = {{0}},
        .coef = {
        {0x0029c9b6, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0},
        {0x0053936d, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0},
        {0x0029c9b6, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0},
        {0x76898c49, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0},
        {-0x3730b323, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0}}
    } 
};

int32_t eq_process(int aid, int32_t new_sample)
{
  return filter_biquads_s32(&eq_peak[aid], 1, new_sample);
}

void eq_confBiquad(int aid,int ch, int32_t coef[5])
{
    for(int i=0; i<5; i++)
    {
        eq_peak[aid].coef[i][ch] = coef[i];
    }
    for(int i=1; i<2; i++)
    {
        for(int j=0; j<9; j++)
        {
            eq_peak[aid].state[i][j] = 0;
        }
    }
}

void eq_setBiquad(int aid, int ch, int type, double Fc, double Q, double peakGain)
{
  double data[5]={0};
  biquad_calcBiquad(type,  Fc,  Q,  peakGain,  data);
  for(int i=0; i<5; i++)
  {
      eq_peak[aid].coef[i][ch] = biquad_float_to_q30_hex(data[i]);
  }
  for(int i=1; i<2; i++)
  {
      for(int j=0; j<9; j++)
      {
         eq_peak[aid].state[i][j] = 0;
      }
  }
}